// Copyright 2008 Isis Innovation Limited
// This is the main extry point for PTAM
#include <stdlib.h> //sistema
#include <iostream> //sistema
#include <gvars3/instances.h> //instalada gvars3
#include <TooN/se3.h>  //instalada TooN
#include "RandomHelpers.h" //en ptam_mixed


#include "System_ros.h"
#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/NavSatFix.h"
#include <tf/transform_listener.h>
#include "geometry_msgs/Quaternion.h"




#include <math.h>

const float RADIANS_PER_DEGREE = M_PI/180.0;

using namespace std;
using namespace GVars3;
bool verbose;


class UAVDATMO{
public:
  UAVDATMO();
  void CAM_Callback(const sensor_msgs::Image::ConstPtr &msg);
  void IMU_Callback(const sensor_msgs::Imu::ConstPtr &msg);
  void GPS_Callback(const sensor_msgs::NavSatFix::ConstPtr &msg);
  ros::NodeHandle n;
  void * user;
  ros::Subscriber cam_sub;
private:
  double imuTimeTolerance;
  double gpsTimeTolerance;
  TooN::Vector<3> v3IgnoreMeasurement; // Any negative values are ignored
  // World has x-axis east, y-axis north, z-axis up (UTM)
  // GPS has same axes as world
  // IMU has x-axis forward, y-axis left, z-axis up
  // Camera has x-axis as image plane right, y-axis as image plane down, z-axis outwards along optical axis
  double offsetTime;
  double imgTime;
  // Camera w.r.t IMU -> only orientation (orientation order is yaw->pitch->roll, +ve angles are anticlockwise when looking into axis)
  double imuTime;
  SO3<> so3IMUFromCam;
  Vector<3> v3ImuMeasNoise;
  SO3<> so3WorldFromIMU;
  Vector<3> v3AngularVelocity;
  // Camera w.r.t GPS -> only position
  double gpsTime;
  Vector<3> v3GPSFromCam;
  TooN::Vector<3> v3GpsMeasNoise;
  TooN::Vector<3> v3WorldFromGPS;
  TooN::Vector<2> v2HorizontalVelocity;
  SO3<> so3GfromIMU;
  TooN::Vector<3> v3GfromGPS;


};

UAVDATMO::UAVDATMO(){
  //Initialization
  imuTimeTolerance = 40000;
  gpsTimeTolerance = 100000;
  v3IgnoreMeasurement = TooN::makeVector(-1,-1,-1); // Any negative values are ignored
  // World has x-axis east, y-axis north, z-axis up (UTM)
  // GPS has same axes as world
  // IMU has x-axis forward, y-axis left, z-axis up
  // Camera has x-axis as image plane right, y-axis as image plane down, z-axis outwards along optical axis
  offsetTime = 0;
  imgTime = 0;
  // Camera w.r.t IMU -> only orientation (orientation order is yaw->pitch->roll, +ve angles are anticlockwise when looking into axis)
  imuTime = 0;
  so3IMUFromCam = PackSO3_RPY(0.0, M_PI, M_PI/2);
  v3ImuMeasNoise = makeVector(0.1,0.1,0.1);

  // Camera w.r.t GPS -> only position
  gpsTime = 0;
  v3GPSFromCam = makeVector(0.0, 0.0, 0.0);

}


/*

void UAVDATMO::IMU_Callback(const sensor_msgs::Imu::ConstPtr &msg)
{
  double r,p,y;
  btQuaternion q;
  q.setX((float)msg->orientation.x);
  q.setY((float)msg->orientation.y);
  q.setZ((float)msg->orientation.z);
  q.setW((float)msg->orientation.w);
  btMatrix3x3(q).getRPY(r,p,y);
  //TooN::SO3<> ej = PackSO3_RPY(r,p,y);

  imuTime = msg->header.stamp.toSec();
  //Repasar velocidades
  so3GfromIMU = PackSO3_RPY(r,p,y);
  //COMPROBAR QUE angvelocity x,y,z coincide con rvel,pvel,yvel.
  v3AngularVelocity = makeVector(msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);

  if (verbose)
    {
      std::cout << "IMU: " << (imuTime-offsetTime)/60000000 << " (" << raw_imu.roll << "," << raw_imu.pitch << "," << raw_imu.yaw << ")" << std::endl;
    }


}
*/

void UAVDATMO::CAM_Callback(const sensor_msgs::Image::ConstPtr &msg)
{

  bool bSyncGPS = false, bSyncIMU = false;
  if(imgTime == 0)
  {
    imgTime = (double)msg->header.stamp.toSec()*1000;
    offsetTime = imgTime;
  }
  else
    imgTime = (double)(double)msg->header.stamp.toSec()*1000;

  SE3<> se3GfromC;
  Vector<6> v6GfromCNoise;
  SE3GaussianPDF se3PdfGfromC;

  if(fabs(imgTime-imuTime) < imuTimeTolerance)
  {
    bSyncIMU = true;
    Vector<3> v3AngularMotion = v3AngularVelocity * (1e-6 * (imgTime - imuTime));
    SO3<> so3AngularMotion = PackSO3_RPY(v3AngularMotion[0], v3AngularMotion[1], v3AngularMotion[2]);
    so3GfromIMU = so3GfromIMU * so3AngularMotion;
    SO3<> so3GfromC = so3GfromIMU * so3IMUFromCam;

    if(fabs(imgTime-gpsTime) < gpsTimeTolerance)
    {
      bSyncGPS = true;
      Vector<2> v2HorizontalMotion = v2HorizontalVelocity * (1e-6 * (imgTime - gpsTime));
      v3GfromGPS.slice(0,2) = v3GfromGPS.slice(0,2) + v2HorizontalMotion;
      Vector<3> v3GfromC = v3GfromGPS + v3GPSFromCam;

      se3PdfGfromC = SE3GaussianPDF(so3GfromC, v3ImuMeasNoise, v3GfromC, v3GpsMeasNoise);
    }
    else
    {
      se3PdfGfromC = SE3GaussianPDF(so3GfromC, v3ImuMeasNoise);
    }
  }

  if(verbose)
    std::cout << "IMG: " << (imgTime-offsetTime)/60000000 << " synched with GPS: " << bSyncGPS << ", IMU: " << bSyncIMU << std::endl;


  System *s = static_cast<System*>(user);

  s->NewObservations(msg, se3PdfGfromC);

}


/*
void UAVDATMO::GPS_Callback(const sensor_msgs::NavSatFix::ConstPtr &msg)
{

  if(msg->status.status > 0) //Position fixed
  {
    gpsTime = msg->header.stamp.toSec();
    //Change long, lat in degrees to east, north UTM in m
    //v3WorldFromGPS = makeVector(msg->longitude,msg->latitude,msg->altitude);

    //v2HorizontalVelocity = makeVector(raw_gps.vx, raw_gps.vy);



    if(verbose)
             {
               std::cout << "GPS: " << (gpsTime-offsetTime)/60000000 << " (" << raw_gps.east << "," << raw_gps.north << "," << raw_gps.alt << ")";
               std::cout << "[" << raw_gps.eph << "," << raw_gps.epv << "]" << std::endl;
             }


  }

//  mavlink_gps_raw_asctec_t raw_gps;
//       mavlink_msg_gps_raw_asctec_decode(msg, &raw_gps);

//       if(raw_gps.status) // Check for a valid gps fix first (we get messages even if there is not)
//       {
//         gpsTime = (double)raw_gps.usec;
//         v3WorldFromGPS = makeVector(raw_gps.east, raw_gps.north, raw_gps.alt);
//         v2HorizontalVelocity = makeVector(raw_gps.vx, raw_gps.vy);
//         v3GpsMeasNoise = makeVector(raw_gps.eph, raw_gps.eph, raw_gps.epv);
//
//         if(verbose)
//         {
//           std::cout << "GPS: " << (gpsTime-offsetTime)/60000000 << " (" << raw_gps.east << "," << raw_gps.north << "," << raw_gps.alt << ")";
//           std::cout << "[" << raw_gps.eph << "," << raw_gps.epv << "]" << std::endl;
//         }
//       }



}
*/


int main(int argc, char **argv)
{
  ros::init(argc,argv,"uavdatmo");
  ROS_INFO("FUNCIONA!");

  UAVDATMO *controller = new UAVDATMO();
  // Detection *controller = new Detection();
  //  float r,p,y;
  //  TooN::SO3<> ej = PackSO3_RPY(r,p,y);
  /*
  //Time string for saving purposes
  time_t now = time(NULL);
  struct tm* now_time = gmtime(&now);
  std::stringstream time_str;
  time_str << now_time->tm_year+1900 <<"_" << now_time->tm_mon+1 <<"_";
  time_str << now_time->tm_mday <<"_" << now_time->tm_hour <<"_";
  time_str << now_time->tm_min <<"_" << now_time->tm_sec <<"_";
  //folder creation
  std::stringstream folder_name;
  folder_name << "./trial_" << time_str.str();
  controller->trial_folder_name = folder_name.str();
  string folder_string = folder_name.str();
  mkdir(folder_string.c_str(),S_IRWXU);
   */
  /*
  controller->n.getParam("Robot_ID",controller->RobotStateData.RobotID);
  controller->n.getParam("Robot_name",controller->RobotStateData.RobotName);
  controller->n.getParam("RangeFinderMaxRange",controller->SensorInfo.maxRange);
  controller->n.getParam("RangeFinderHorFoV",controller->SensorInfo.horFoV);
  controller->n.getParam("RangeFinderVerFoV",controller->SensorInfo.verFoV);
  ROS_INFO("PFUNCIONA!3333");
   */
  //ROS Subscribers Creation



  cout << "  You are running SPTAM: " << endl;
  cout << "  Superior Parallel tracking and mapping for large workspaces" << endl;

  GUI.LoadFile("./config/settings.cfg");
  cout << " After Loading" << endl;
  static gvar3<int> gvnVerbose("System.Verbose", 0, HIDDEN|SILENT);
  static gvar3<int> gvnUseGUI("System.UseGUI", 1, HIDDEN|SILENT);

  static gvar3<int> gvnGlobalSO3("System.GlobalSO3", 0, HIDDEN|SILENT);
  if(*gvnGlobalSO3)
    cout << "GlobalSO3: YES" << endl;
  else
    cout << "GlobalSO3: NO"  << endl;

  static gvar3<int> gvnGlobalSE3("System.GlobalSE3", 0, HIDDEN|SILENT);
  if(*gvnGlobalSE3)
    cout << "GlobalSE3: YES" << endl;
  else
    cout << "GlobalSE3: NO"  << endl;


  if(*gvnUseGUI)
  {
    GUI.StartParserThread(); // Start parsing of the console input
    atexit(GUI.StopParserThread);
  }

  if(*gvnVerbose)
    verbose = true;
  else
    verbose = false;
  try
  {

    System s(*gvnUseGUI);
    controller->user = &s;
    controller->cam_sub = controller->n.subscribe("gscam/image_raw",1000,&UAVDATMO::CAM_Callback,controller);

    //  ros::Subscriber imu_sub = controller->n.subscribe("imu",1000,&UAVDATMO::IMU_Callback,controller);
    //  ros::Subscriber gps_sub = controller->n.subscribe("gps",1000,&UAVDATMO::GPS_Callback,controller);

      ros::spin();
  }
  catch(CVD::Exceptions::All e)
  {
    cout << "ERROR: Failed to run system, got exception: " << e.what << endl;
  }

  std::cout << "Problema" << std::endl;
  delete controller;
  return 0;


}
